/**
* @file main.c
* @brief 云台无刷电机FOC控制程序
* @version V1.0
* @author HuiTailang
* @date 2023年8月
*/
#include "stm32f10x.h"
#include <stdlib.h>
#include "main.h"
#include "led.h"

float target=0;

void commander_run(void);

int main(void)
{
	unsigned int count_i=0;
    LED_Init();
	
	uart_init(115200);
	
	I2C_Init_();               //AS5600
	printf("AS5600\r\n");

	TIM2_PWM_Init();
	TIM3_1ms_Init();           //interrupt per 1ms
	
	delay_ms(1000);            //Wait for the system to stabilize
	MagneticSensor_Init();     //AS5600 or TLE5012B
	Voltage_Init();
	velocity_limit=20;         //rad/s angleOpenloop() and PID_angle() use it
	torque_controller=Type_voltage;  //当前只有电压模式
	controller=Type_angle;  //Type_angle; //Type_torque;    //
	
	Motor_init();
	Motor_initFOC();
	PID_init();
    printf("Motor ready.\r\n");
	
	systick_CountMode();   //不能再调用delay_us()和delay_ms()函数
	
	while(1)
	{
		LED1_OFF();
		count_i++;
		
		if(time1_cntr>=200)  //0.2s
		{
			time1_cntr=0;
			LED1_ON();
		}
		if(time2_cntr>=1000)
		{
			time2_cntr=0;
			
			printf("%d\r\n",count_i);
			count_i=0;
		}
		move(target);
		loopFOC();
		commander_run();
	}
}

/**
  * @brief  串口接收数据处理
  * @param  None 
  * @retval None  
  */ 
void commander_run(void)
{
	if((USART_RX_STA&0x8000)!=0)
	{
		switch(USART_RX_BUF[0])
		{
			case 'H':
				printf("Hello World!\r\n");
				break;
			case 'T':   //T6.28
				target=atof((const char *)(USART_RX_BUF+1));
				printf("RX=%.4f\r\n", target);
				break;
			case 'D':   //D
				M1_Disable();
				printf("OK!\r\n");
				break;
			case 'E':   //E
				M1_Enable();
				printf("OK!\r\n");
				break;
		}
		USART_RX_STA=0;
	}
}
/******************************************************************************/



